#include "ultra_band_wave_ladar/Sensor_RadLogic.hpp"

Sensor_RadLogic::Sensor_RadLogic()
{
  InitPara();

  mmRad_front.setScanData(sensorinfo.array_mmRad);
  UltraRad_back.setScanData(sensorinfo.array_UltraRad_back);
  UltraRad_left.setScanData(sensorinfo.array_UltraRad_left);
  UltraRad_right.setScanData(sensorinfo.array_UltraRad_right);
}

Sensor_RadLogic::~Sensor_RadLogic()
{
}

void Sensor_RadLogic::InitPara()
{
  sensorinfo.mmRad_front.distance = 0;
  sensorinfo.mmRad_front.maplitude = 0;
  sensorinfo.mmRad_front.obs_angle = 0;
  sensorinfo.mmRad_front.obs_lenth = 0;
  sensorinfo.mmRad_front.obs_width = 0;

  sensorinfo.mmRad_front.distance = 0;
  sensorinfo.mmRad_front.maplitude = 0;
  sensorinfo.mmRad_front.obs_angle = 0;
  sensorinfo.mmRad_front.obs_lenth = 0;
  sensorinfo.mmRad_front.obs_width = 0;

  sensorinfo.UltraRad_left.distance = 0;
  sensorinfo.UltraRad_left.maplitude = 0;
  sensorinfo.UltraRad_left.obs_angle = 0;
  sensorinfo.UltraRad_left.obs_lenth = 0;
  sensorinfo.UltraRad_left.obs_width = 0;

  sensorinfo.UltraRad_right.distance = 0;
  sensorinfo.UltraRad_right.maplitude = 0;
  sensorinfo.UltraRad_right.obs_angle = 0;
  sensorinfo.UltraRad_right.obs_lenth = 0;
  sensorinfo.UltraRad_right.obs_width = 0;

  sensorinfo.machine_st = 0;
  sensorinfo.run_st = 0;

  sensorinfo.mmRad_Alarm = false;
  sensorinfo.UltraRad_back_Alarm = false;
  sensorinfo.UltraRad_left_Alarm = false;
  sensorinfo.UltraRad_right_Alarm = false;

  sensorinfo.array_mmRad.clear();
  sensorinfo.array_UltraRad_back.clear();
  ;
  sensorinfo.array_UltraRad_left.clear();
  sensorinfo.array_UltraRad_right.clear();
}

void Sensor_RadLogic::setSensorportnames(const std::vector<std::string>& names)
{
  portnames = names;  // 0-mmrad; 1-ultra_back; 2-ultra_left; 3-ultra_right;
}

bool Sensor_RadLogic::InitPort()
{
  if (mmRad_front.Init_mmRadComPort(portnames[0]) && UltraRad_right.Init_UltraRadComPort(portnames[1]) &&
      UltraRad_back.Init_UltraRadComPort(portnames[2]) && UltraRad_left.Init_UltraRadComPort(portnames[3]))
  {
    SaveDist[0] = 200;  // cm-mmRad
    SaveDist[1] = 200;  // cm-UltraRad_back
    SaveDist[2] = 200;  // cm-UltraRad_left
    SaveDist[3] = 200;  // cm-UltraRad_right

    return true;
  }
  else
    return false;
}

void Sensor_RadLogic::ActiveSensor()
{
  set_AlarmDistance(&SaveDist[0]);
  mmRad_front.mmRadScan_Start();
  UltraRad_back.UltraRadScan_Start();
  UltraRad_left.UltraRadScan_Start();
  UltraRad_right.UltraRadScan_Start();
}

void Sensor_RadLogic::setRadMode(int type, int Var, int side)
{
  switch (side)
  {
    case 0:
      mmRad_front.set_CommandMode(type, Var);
      break;
    case 1:
      UltraRad_back.set_CommandMode(type, Var);
      break;
    case 2:
      UltraRad_left.set_CommandMode(type, Var);
      break;
    case 3:
      UltraRad_right.set_CommandMode(type, Var);
      break;
    default:
      break;
  }
}

void Sensor_RadLogic::set_AlarmDistance(int* Dist)
{
  mmRad_front.set_AlarmDistance(Dist[0]);
  UltraRad_back.set_AlarmDistance(Dist[1]);
  UltraRad_left.set_AlarmDistance(Dist[2]);
  UltraRad_right.set_AlarmDistance(Dist[3]);
}

void Sensor_RadLogic::mmRadProcessing()
{
  // mmRad_front.get_mmRadComScanData();
  if (!sensorinfo.array_mmRad.empty())
  {
    mmRad_front.setScanData(sensorinfo.array_mmRad);
    if (mmRad_front.ObstacleScan())
    {
      sensorinfo.mmRad_front.distance = mmRad_front.get_ActualDistance();
      sensorinfo.mmRad_front.maplitude = mmRad_front.get_ActualAmplitude();
      sensorinfo.mmRad_Alarm = true;
    }
  }
}

void Sensor_RadLogic::UltraRadProcessing()
{
  // mmRad_front.get_mmRadComScanData();
  if (!sensorinfo.array_UltraRad_back.empty())
  {
    UltraRad_back.setScanData(sensorinfo.array_UltraRad_back);
    if (UltraRad_back.ObstacleScan())
    {
      sensorinfo.UltraRad_back.distance = UltraRad_back.get_ActualDistance();
      sensorinfo.UltraRad_back.maplitude = UltraRad_back.get_ActualAmplitude();
      sensorinfo.UltraRad_back_Alarm = true;
    }
  }

  if (!sensorinfo.array_UltraRad_left.empty())
  {
    UltraRad_left.setScanData(sensorinfo.array_UltraRad_left);
    if (UltraRad_back.ObstacleScan())
    {
      sensorinfo.UltraRad_left.distance = UltraRad_left.get_ActualDistance();
      sensorinfo.UltraRad_left.maplitude = UltraRad_left.get_ActualAmplitude();
      sensorinfo.UltraRad_left_Alarm = true;
    }
  }

  if (!sensorinfo.array_UltraRad_right.empty())
  {
    UltraRad_right.setScanData(sensorinfo.array_UltraRad_right);
    if (UltraRad_right.ObstacleScan())
    {
      sensorinfo.UltraRad_right.distance = UltraRad_right.get_ActualDistance();
      sensorinfo.UltraRad_right.maplitude = UltraRad_right.get_ActualAmplitude();
      sensorinfo.UltraRad_right_Alarm = true;
    }
  }
}

void Sensor_RadLogic::ObstacleProcessing()
{
  //
}

void Sensor_RadLogic::ClosePort()
{
  mmRad_front.CloseComPort();
  UltraRad_back.CloseComPort();
  UltraRad_left.CloseComPort();
  UltraRad_right.CloseComPort();
}

int main(int argc, char** argv)
{
  Sensor_RadLogic radlogic;
  ros::init(argc, argv, "Sensor_Randor_node");
  ros::NodeHandle nh;

  if (sensorinfo.machine_st == 1)
  {
    while (!sensorinfo.array_portnames.empty())
    {
      radlogic.setSensorportnames(sensorinfo.array_portnames);
      if (radlogic.InitPort())
      {
        radlogic.setRadMode(0, 0, 0);  // mmrad
        radlogic.setRadMode(0, 0, 1);  // ultrarad_back
        radlogic.setRadMode(0, 0, 2);  // ultrarad_left
        radlogic.setRadMode(0, 0, 3);  // ultrarad_right
      }
    }
    ros::Rate loop_rate(50);
    if (sensorinfo.run_st == 1)
    {
      radlogic.ActiveSensor();
      while (ros::ok())
      {
        radlogic.mmRadProcessing();
        radlogic.UltraRadProcessing();
        // radlogic.ObstacleProcessing();
        ros::spinOnce();
        loop_rate.sleep();
      }
    }
    else
    {
      //
    }
  }
  else
  {
    radlogic.ClosePort();
    radlogic.InitPara();
  }

  return 0;
}
